/*******************************************************************************
 *
 *      Filename: test_mat3.cpp
 *   Description: Unit test for mat3 class
 *        Author: [kb]ypp
 *    Created at: November 4, 2005
 * Last modified: 
 *
 ******************************************************************************/

#include "../Matrix3x3.h"
#include "../Vector3.h"
#include <boost/test/unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
using namespace boost::unit_test;
using boost::test_toolbox::close_at_tolerance;
#include <cmath>
using std::sqrt;

const float TOLERANCE = 0.0001;

#include <iostream>
using namespace std;

ostream& operator<<( ostream &os, const mat3 &m )
{
	return os << "( " << m.m11 << ' ' << m.m12 << ' ' << m.m13 << "; "
					  << m.m21 << ' ' << m.m22 << ' ' << m.m23 << "; "
					  << m.m31 << ' ' << m.m32 << ' ' << m.m33
			  << " )";
}

void constructors_test()
{
	// Use the default constructor:
	mat3 dflt;
	
	// Test the constructor with 9 float`s:
	mat3 m(  1.1, -1.2,  1.3,
			-2.1,  2.2, -2.3,
			 3.1, -3.2,  3.3 );
	BOOST_CHECK_CLOSE( m.m11,  1.1f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m12, -1.2f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m13,  1.3f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m21, -2.1f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m22,  2.2f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m23, -2.3f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m31,  3.1f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m32, -3.2f, TOLERANCE );
	BOOST_CHECK_CLOSE( m.m33,  3.3f, TOLERANCE );
	
	// Test the copy-constructor:
	mat3 mCopy( m );
	BOOST_CHECK_CLOSE( mCopy.m11, m.m11, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m12, m.m12, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m13, m.m13, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m21, m.m21, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m22, m.m22, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m23, m.m23, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m31, m.m31, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m32, m.m32, TOLERANCE );
	BOOST_CHECK_CLOSE( mCopy.m33, m.m33, TOLERANCE );
}

void assignment_test()
{
	// Test operator=()
	const mat3 m(  1.1, -1.2,  1.3,
				  -2.1,  2.2, -2.3,
				   3.1, -3.2,  3.3 );
	mat3 mAssign;
	mAssign = m;
	BOOST_CHECK_CLOSE( mAssign.m11, m.m11, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m12, m.m12, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m13, m.m13, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m21, m.m21, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m22, m.m22, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m23, m.m23, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m31, m.m31, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m32, m.m32, TOLERANCE );
	BOOST_CHECK_CLOSE( mAssign.m33, m.m33, TOLERANCE );
	
	/*const mat3 v1( 1.0, 2.123, 4.5 );
	const mat3 v2( 0.0, -0.56, 1.25 );
	
	// Test operator+=()
	mat3 v12;
	v12 = v1;
	v12 += v2;
	BOOST_CHECK_CLOSE( v12.x, v1.x + v2.x, TOLERANCE );
	BOOST_CHECK_CLOSE( v12.y, v1.y + v2.y, TOLERANCE );
	BOOST_CHECK_CLOSE( v12.z, v1.z + v2.z, TOLERANCE );

	// Test operator-=()
	mat3 v21;
	v21 = v2;
	v21 -= v1;
	BOOST_CHECK_CLOSE( v21.x, v2.x - v1.x, TOLERANCE );
	BOOST_CHECK_CLOSE( v21.y, v2.y - v1.y, TOLERANCE );
	BOOST_CHECK_CLOSE( v21.z, v2.z - v1.z, TOLERANCE );*/
}

void arithmetic_test()
{
	const mat3 M( 1.1, 1.2, 1.3,
				  2.1, 2.2, 2.3,
				  3.1, 3.2, 3.3 );
	const mat3 Q( 1.0, 2.0, 3.0,
				  0.0, 1.0, 2.0,
				  0.0, 0.0, 1.0 );
	const mat3 P( 1.0, 5.0, 3.0,
				  0.0, 2.0, 7.0,
				  0.0, 0.0, 3.0 );
	
	// Test operator*( matrix, matrix )
	BOOST_CHECK_EQUAL( M * Q, mat3( 1.1, 3.4,  7.0,
									2.1, 6.4, 13.0,
									3.1, 9.4, 19.0 ) );
	BOOST_CHECK_EQUAL( Q * P, mat3( 1.0, 9.0, 26.0,
									0.0, 2.0, 13.0,
									0.0, 0.0,  3.0 ) );
	BOOST_CHECK_EQUAL( M * P * Q, mat3( 1.1, 10.1, 34.7,
										2.1, 19.1, 64.7,
										3.1, 28.1, 94.7 ) );
	BOOST_CHECK_EQUAL( M * Q * P, mat3( 1.1, 12.3,  48.1,
										2.1, 23.3,  90.1,
										3.1, 34.3, 132.1 ) );
	
	// Test operator*( matrix, vector )
	const vec3 v( 0.1, 1.2, 2.3 );
	const vec3 v1( 1.0, 2.0, 3.0 );
	const vec3 v2( 7.0, 6.0, 5.0 );
	
	BOOST_CHECK( M * v == vec3( 4.54, 8.14, 11.74 ) );
	BOOST_CHECK( Q * v1 == vec3( 14.0, 8.0, 3.0 ) );
	BOOST_CHECK( P * v2 == vec3( 52.0, 47.0, 15.0 ) );
	BOOST_CHECK( M * vec3::ZERO == vec3::ZERO );
}

void comparison_test()
{
	const mat3 m1(  1.1, -1.2,  1.3,
				   -2.1,  2.2, -2.3,
				    3.1, -3.2,  3.3 );
	mat3 m2(  1.1, -1.2,  1.3,
			 -2.1,  2.2, -2.3,
			  3.1, -3.2,  3.3 );
	
	// Test operator==()
	BOOST_CHECK( m1 == mat3(  1.1, -1.2,  1.3,
							 -2.1,  2.2, -2.3,
							  3.1, -3.2,  3.3 ) );
	BOOST_CHECK( mat3(  1.1, -1.2,  1.3,
					   -2.1,  2.2, -2.3,
					    3.1, -3.2,  3.3 ) == m1 );
	BOOST_CHECK( m1 == m2 );
	
	m2.m22 = 0.0;
	
	// Test operator!=()
	BOOST_CHECK( m2 != mat3(  1.1, -1.2,  1.3,
							 -2.1,  2.2, -2.3,
							  3.1, -3.2,  3.3 ) );
	BOOST_CHECK( mat3(  1.1, -1.2,  1.3,
					   -2.1,  2.2, -2.3,
					    3.1, -3.2,  3.3 ) != m2 );
	BOOST_CHECK( m1 != m2 );
	
	BOOST_CHECK( m2 == mat3(  1.1, -1.2,  1.3,
							 -2.1,  0.0, -2.3,
							  3.1, -3.2,  3.3 ) );
}

void other_test()
{
	const mat3 M( 1.1, 1.2, 1.3,
				  2.1, 2.2, 2.3,
				  3.1, 3.2, 3.3 );

	// Test loadIdentity()
	const mat3 I( 1.0, 0.0, 0.0,
				  0.0, 1.0, 0.0,
				  0.0, 0.0, 1.0 );
	mat3 A( M );
	BOOST_CHECK( A != I );
	A.loadIdentity();
	BOOST_CHECK_EQUAL( A, I );

	const mat3 Q( 1.0, 2.0, 3.0,
				  0.0, 1.0, 2.0,
				  0.0, 0.0, 1.0 );
	const mat3 P( 1.0, 5.0, 3.0,
				  0.0, 2.0, 7.0,
				  0.0, 0.0, 3.0 );
	
	// Test det()
	BOOST_CHECK( fabs( det( M ) + 2.22044604925031e-017f ) < TOLERANCE );
	BOOST_CHECK_CLOSE( det( Q ), 1.0f, TOLERANCE );
	BOOST_CHECK_CLOSE( det( P ), 6.0f, TOLERANCE );
	BOOST_CHECK_CLOSE( det( Q * P ), 6.0f, TOLERANCE );
	
	// Test transpose()
	mat3 Mt = transpose( M );
	BOOST_CHECK_CLOSE( Mt.m11, M.m11, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m12, M.m21, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m13, M.m31, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m21, M.m12, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m22, M.m22, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m23, M.m32, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m31, M.m13, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m32, M.m23, TOLERANCE );
	BOOST_CHECK_CLOSE( Mt.m33, M.m33, TOLERANCE );
	BOOST_CHECK_EQUAL( I, transpose( I ) );
	
	// Test inv()
	inv( M ); // Log message expected !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
	BOOST_CHECK_EQUAL( inv( Q ), mat3( 1.0, -2.0,  1.0,
									   0.0,  1.0, -2.0,
									   0.0,  0.0, 1.0 ) );
	BOOST_CHECK_EQUAL( inv( P ), mat3( 1.0, -2.5,  4.8333,
									   0.0,  0.5, -1.1667,
									   0.0,  0.0,  0.3333 ) );
	BOOST_CHECK_EQUAL( inv( Q * P ), mat3( 1.0, -4.5, 10.8333,
										   0.0,  0.5, -2.1667,
										   0.0,  0.0,  0.3333 ) );
}

void statics_test()
{
	const float theta = 0.79;
	
	// Test mat3::rotateX()
	BOOST_CHECK_EQUAL( mat3::rotateX( theta ), mat3(
		1.0,          0.0,           0.0,
		0.0, cos( theta ), -sin( theta ),
		0.0, sin( theta ),  cos( theta )
	) );
	
	// Test mat3::rotateY()
	BOOST_CHECK_EQUAL( mat3::rotateY( theta ), mat3(
		 cos( theta ), 0.0, sin( theta ),
		          0.0, 1.0,          0.0,
		-sin( theta ), 0.0, cos( theta )
	) );
	
	// Test mat3::rotateZ()
	BOOST_CHECK_EQUAL( mat3::rotateZ( theta ), mat3(
		 cos( theta ), -sin( theta ), 0.0,
		 sin( theta ),  cos( theta ), 0.0,
		          0.0,           0.0, 1.0
	) );
	
	// Test mat3::rotateAxis()
	vec3 axis( -0.12, 0.0, 0.53 );
	axis.normalize();
	const float c = cos( theta );
	const float s = sin( theta );
	const float x = axis.x, y = axis.y, z = axis.z;
	BOOST_CHECK_EQUAL( mat3::rotateAxis( axis, theta ), mat3(
		  c + (1.0-c) * x*x, (1.0-c) * x*y - s*z, (1.0-c) * x*z + s*y,
		(1.0-c) * x*y + s*z,   c + (1.0-c) * y*y, (1.0-c) * y*z - s*x,
		(1.0-c) * x*z - s*y, (1.0-c) * y*z + s*x,   c + (1.0-c) * z*z
	) );
}

test_suite* init_unit_test_suite( int argc, char* argv[] )
{
	test_suite *test = BOOST_TEST_SUITE( "Unit tests for mat3 class" );
	
	test->add( BOOST_TEST_CASE( &constructors_test ) );
	test->add( BOOST_TEST_CASE( &assignment_test ) );
	test->add( BOOST_TEST_CASE( &arithmetic_test ) );
	test->add( BOOST_TEST_CASE( &comparison_test ) );
	test->add( BOOST_TEST_CASE( &other_test ) );
	test->add( BOOST_TEST_CASE( &statics_test ) );
	
	return test;
}
